package com.suray.wcs.service.thread;

import com.google.common.collect.Lists;
import com.suray.basic.wcs.node.WCSNodeModule;
import com.suray.basic.wcs.path.core.Lifter;
import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.path.core.Route;
import com.suray.basic.wcs.path.core.api.impl.DifFloorRouteFactoryImp;
import com.suray.basic.wcs.path.enums.NodeStatus;
import com.suray.basic.wcs.path.enums.NodeType;
import com.suray.basic.wcs.path.exception.CreatePathFailedException;
import com.suray.basic.wcs.plc.core.Plc;
import com.suray.basic.wcs.plc.enums.Direction;
import com.suray.basic.wcs.plc.enums.PlcDeviceType;
import com.suray.basic.wcs.plc.pojo.DeviceBase;
import com.suray.basic.wcs.plc.pojo.oper.lifter.LifterOper;
import com.suray.basic.wcs.plc.pojo.oper.lifter.TwoDirectionRgvLifterOper;
import com.suray.basic.wcs.plc.utils.PlcUnit;
import com.suray.basic.wcs.time.WCSTimeModule;
import com.suray.basic.wcs.utils.Action;
import com.suray.basic.wcs.utils.Coord;
import com.suray.basic.wcs.utils.LoggerUtil;
import com.suray.basic.wcs.utils.WcsFuncUtil;
import com.suray.commin.core.utils.SpringContextHolder;
import com.suray.commin.exception.OrderExecuteException;
import com.suray.commin.exception.Rgv60TaskException;
import com.suray.system.run.agv.ToUpper;
import com.suray.system.run.agv.bean.StationRunBean;
import com.suray.wcs.res.enums.PalletStatus;
import com.suray.wcs.res.enums.RgvType;
import com.suray.wcs.service.core.path.api.DeadlockHandle;
import com.suray.wcs.service.core.path.api.impl.PathHandleImpl;
import com.suray.wcs.service.core.path.api.impl.RgvSonNewTaskHandle;
import com.suray.wcs.service.core.util.CommonUtil;
import com.suray.wcs.service.core.util.LockNodeManager;
import com.suray.wcs.service.enums.*;
import com.suray.wcs.service.execption.*;
import com.suray.wcs.service.init.SystemInit;
import com.suray.wcs.service.plc.LifterNodeAnalyze;
import com.suray.wcs.service.plc.LifterPathNode;
import com.suray.wcs.service.plc.util.LifterEventInterface;
import com.suray.wcs.service.pojo.Rgv;
import com.suray.wcs.service.pojo.Rgv60;
import com.suray.wcs.service.pojo.RgvSM;
import com.suray.wcs.service.pojo.rgv.action.util.RgvTaskMode;
import com.suray.wcs.service.pojo.rgv.rgv.protocol.util.RgvCargoStatus;
import com.suray.wcs.service.rescontact.ActionHandle;
import com.suray.wcs.service.rescontact.ToRes;
import com.suray.wcs.service.system.entity.*;
import com.suray.wcs.service.system.service.impl.*;
import com.suray.wcs.service.util.WcsUtil;
import com.suray.wcs.service.wmsconnect.TaskStatusRtn;
import com.suray.wcs.service.wmsconnect.ToWMSImpl;
import org.apache.commons.lang3.StringUtils;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.context.annotation.Lazy;
import org.springframework.stereotype.Component;

import java.io.IOException;
import java.util.*;

@Lazy
@Component
public class RgvTaskHandle {
    private Logger log = LoggerFactory.getLogger(this.getClass());

    @Autowired
    private PathHandleImpl pathHandle;
    @Autowired
    private TaskDBServiceImpl taskDBService;
    @Autowired
    private NodeDBServiceImpl nodeDBService;
    @Autowired
    private WareHouseDBServiceImpl wareHouseDBService;
    @Autowired
    private TaskDetailServiceImpl taskDetailService;

    private ToWMSImpl toWMS = SpringContextHolder.getBean("toWMSImpl");

    @Autowired
    private PlcDBServiceImpl plcDBService;

    @Autowired
    private DeadlockHandle deadlockHandle;

    /**
     * 路径处理接口
     *
     * @throws InterruptedException
     * @throws PlcExecuteException
     * @throws IOException
     */
    public void handle(TaskDB taskDB, TaskDetailDB rgvTaskDetail) throws InterruptedException, CreatePathFailedException, PlcExecuteException, IOException {
        Rgv rgv = taskDB.selectProcessingRgvPojo();
        ToRes toRes = ToRes.getInstance(rgv);
        WCSNodeModule.cleanWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo());
        WareHouseDB wareHouseDB = wareHouseDBService.getCurWareHouseDB();
        // 执行任务前校验条码值
        if (rgv.getGroundCode() != null) {
            //有条码
            NodeDB nodedb;
            if (rgv.checkRgvType(RgvType.RGV_SEER_COMM)) {
                nodedb = nodeDBService.getNodeDB(rgv.getCurLocation());
            } else {
                nodedb = nodeDBService.findByBarcode(rgv.getGroundCode());
            }

            // 存在这个条码对应的位置
            if (nodedb != null) {
                if (!nodedb.getCanToStop()) {
                    throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), 255, "不可停留点不可作为起点！");
                }
                if (!nodedb.getGridType().equals(NodeType.L)) {
                    if (rgv.getRgvTaskMode() == RgvTaskMode.NORMAL) {
                        if (nodedb.getGridx() != rgv.getCurLocation().getX() || nodedb.getGridy() != rgv.getCurLocation().getY()
                                || nodedb.getGridz() != rgv.getCurLocation().getZ()) {
                            throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), 255, "小车当前位置" + rgv.getCurLocation().toString()
                                    + "与扫到的条码值" + rgv.getGroundCode() + "的位置(" + nodedb.getGridx() + "," + nodedb.getGridy() + "," + nodedb.getGridz() + ")不一致");
                        }
                    } else {
                        LoggerUtil.info("小车当前任务模式不检查地码：" + rgv.getRgvTaskMode(), this.getClass());
                    }
                } else {
                    LifterOper hoistOper = PlcUnit.getLifterOper(new Coord(nodedb.getGridx(), nodedb.getGridy(), nodedb.getGridz()));
                    if (rgv.getCurLocation().getZ() != hoistOper.getHoistCurrentLayer() || !hoistOper.isArrived()) {
                        throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), 256, "小车的定位和提升机的层数不一致，提升机层" + hoistOper.getHoistCurrentLayer());
                    }
                    if (nodedb.getGridx() != rgv.getCurLocation().getX() || nodedb.getGridy() != rgv.getCurLocation().getY()) {
                        throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), 255, "小车当前位置" + rgv.getCurLocation().toString()
                                + "与扫到的条码值" + rgv.getGroundCode() + "的位置(" + nodedb.getGridx() + "," + nodedb.getGridy() + "," + nodedb.getGridz() + ")不一致");
                    }
                }
            } else {
                if (rgv.getRgvTaskMode() == RgvTaskMode.NORMAL) {
                    throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), 255, "小车当前位置" + rgv.getCurLocation().toString()
                            + "扫到的条码值" + rgv.getGroundCode() + "在数据库中不存在！");
                }
            }
        } else {
            throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), 255, "小车当前位置" + rgv.getCurLocation().toString()
                    + "未扫描到条码无法运行！");
        }
        long sendTime = System.currentTimeMillis();
        rgv.checkInterrupt("RgvTaskHandle7", sendTime);
        taskDBService.updateTaskStatus(taskDB, TaskStatus.ON_GOING);
        //先规划从小车位置到任务起点位置的路径 执行去取货路径
        if (taskDB.selectStartNode() != null) {
            //取货执行之后才是冲往终点的状态
            if (taskDB.getRgvProgress().getVal() <= TaskRgvProgress.RGV_TO_START.getVal()
                    && taskDB.getLifterPath() == null) {
                TaskDetailDB runToStartDetail = taskDetailService.getTaskDetail(taskDB.getTaskId(), DetailProcess.RGV_TO_START, rgvTaskDetail.getUuid(),
                        rgv.getCurLocationStr(), taskDB.getStartNodeStr(), PrecessStatus.CONDUCT, rgv.toString(), null);
                runToStart(taskDB, rgv, toRes, runToStartDetail, wareHouseDB);
                taskDetailService.updateTaskDetailCompleteTime(runToStartDetail);
                Set<Coord> unlockSet = LockNodeManager.getLockCoordByObj(rgv);
                unlockSet.removeAll(nodeDBService.getLinkedNode(rgv.getCurLocation()));
                log.info("需要释放的路径资源为：" + unlockSet);
                LockNodeManager.removeLockedNodes(unlockSet, rgv);
                if (taskDB.getTaskType().equals(TaskType.OUT) || taskDB.getTaskType().equals(TaskType.IN) || taskDB.getTaskType().equals(TaskType.TRANSFER)) {
                    if (StringUtils.isNotBlank(taskDB.getWmsTaskId())) {
                        toWMS.executionPhase(TaskStatusRtn.buildTaskStatusRtn(taskDB.getWmsTaskId(), WmsTaskStatus.TAKE_FINISH));
                    }
                }
            }
        }
        rgv.checkInterrupt("RgvTaskHandle8", sendTime);
        //如果是出库移库任务，且当前小车进度为前往起点中，则判断是否有等待的任务，如果有，则让路状态
        if ((TaskType.OUT.equals(taskDB.getTaskType()) || TaskType.TRANSFER.equals(taskDB.getTaskType()))
                && TaskRgvProgress.RGV_TO_START.equals(taskDB.getRgvProgress())) {
            NodeDB nodeDB = nodeDBService.getNodeDB(rgv.getCurLocation());
            if (nodeDBService.isSingleRoad(nodeDB) || nodeDBService.isDoubleRoad(nodeDB)) {
                List<Node> nextPathNodes = Lists.newArrayList(rgv.getCurLocation());
                if (pathHandle.checkSingleRoadHaveOtherTaskOut(rgv, nextPathNodes, taskDB, rgvTaskDetail, rgv.getCurLocation(), sendTime)) {
                    rgv.checkInterrupt("PathHandleImpl-SingleRoadHaveOtherTask0", sendTime);
                    log.info("{}暂不可执行，巷道里有未完成的任务: ", rgv.getRgvName());
                    if (rgv.getWaitRgv() != null) {
                        deadlockHandle.waitAvoid();
                    } else {
                        // 重新规划路径
                        log.info("{}暂不可执行,重新规划路径,巷道里有未完成的任务,执行先到先执行逻辑，任务{}车辆开始调整", rgv.getRgvName(), taskDB);
                        throw new OutAdjustException(String.format("%s暂不可执行,重新规划路径,巷道里有未完成的任务,执行先到先执行逻辑，任务%s车辆开始调整", rgv.getRgvName(), taskDB));
                    }
                }
            }
        }

        //子车前往取货点取货
        if (taskDB.getRgvProgress().getVal() <= TaskRgvProgress.RGV_SON_LEAVE_GET.getVal()) {
            if (checkIsSonTaskProcess(rgv, taskDB, wareHouseDB)) {
                Integer resTaskNo = toRes.getAutoIncrementTaskNum();
                RgvSonNewTaskHandle.sonRgvNewRun((RgvSM) rgv, taskDB, resTaskNo, analyzeSonTaskPath(rgv, taskDB),
                        nodeDBService, taskDBService);
                rgv.setCurResTaskNo(null);
            }
        }

        //自卸式提升机移库任务时调用提升机
        lifterCarAnalyze(taskDB, rgv, toRes, rgvTaskDetail, wareHouseDB);
        TaskDetailDB runToEndDetail = taskDetailService.getTaskDetail(taskDB.getTaskId(), DetailProcess.RGV_TO_END, rgvTaskDetail.getUuid(),
                rgv.getCurLocationStr(), taskDB.getEndNodeStr(), PrecessStatus.CONDUCT, rgv.toString(), null);

        //取货完成,规划去放货路径
        if (checkIsSonTaskProcess(rgv, taskDB, wareHouseDB)) {
            Integer resTaskNo = toRes.getAutoIncrementTaskNum();
            RgvSonNewTaskHandle.sonRgvNewRun((RgvSM) rgv, taskDB, resTaskNo, analyzeSonTaskPath(rgv, taskDB),
                    nodeDBService, taskDBService);
            rgv.setCurResTaskNo(null);
        } else {
            if (taskDB.getRgvProgress() != TaskRgvProgress.RGV_COMPLETE) {
                runToEnd(taskDB, rgv, toRes, runToEndDetail, wareHouseDB);
            }
        }

        taskDetailService.updateTaskDetailCompleteTime(runToEndDetail);
        if (taskDB.getTaskType().equals(TaskType.OUT) || taskDB.getTaskType().equals(TaskType.IN) || taskDB.getTaskType().equals(TaskType.TRANSFER)) {
            if (StringUtils.isNotBlank(taskDB.getWmsTaskId())) {
                toWMS.executionPhase(TaskStatusRtn.buildTaskStatusRtn(taskDB.getWmsTaskId(), WmsTaskStatus.PUT_FINISH));
            }
        }
        // 如果是5代车且是出入移，则更新小车身上无货
        if (rgv.checkRgvType(RgvType.FIVE_POINT_FOUR_TL)
                && (TaskType.IN.equals(taskDB.getTaskType())
                || TaskType.OUT.equals(taskDB.getTaskType())
                || TaskType.TRANSFER.equals(taskDB.getTaskType()))) {
            rgv.setCargoTaskId(null);
        }
        taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_COMPLETE);
        if (!taskDB.getTaskType().equals(TaskType.OUT)) {
            taskDBService.updateTaskStatus(taskDB, TaskStatus.FINISH);
            String value = Plc.PLC_INTERRUPT_CACHE.get(String.valueOf(taskDB.getTaskId()));
            if (!StringUtils.isEmpty(value)) {
                Plc.PLC_INTERRUPT_CACHE.remove(String.valueOf(taskDB.getTaskId()));
            }
            // 当前任务有后置任务，把当前任务的后置任务设置为可预分车
            TaskDB sufTask = taskDBService.findTaskByPreTaskId(taskDB.getTaskId());
            if (sufTask != null) {
                // 后置任务是入库任务，开始执行plc流程
                if (TaskType.IN.equals(sufTask.getTaskType())) {
                    PlcTaskThread.getInstance(sufTask);
                } else {
                    taskDBService.updateIsAllowAssignmentRgv(sufTask, true);
                }
            }
            if (StringUtils.isNotBlank(taskDB.getWmsTaskId())) {
                NodeDB nodeDBEnd = nodeDBService.getNodeDB(Node.parseGrid(taskDB.getEndNodeStr()));
                toWMS.executionPhase(TaskStatusRtn.buildTaskStatusRtn(taskDB.getWmsTaskId(), nodeDBEnd.getGridId(), WmsTaskStatus.FINISH));
            }
        }
        Set<Coord> unlockSet = LockNodeManager.getLockCoordByObj(rgv);
        unlockSet.removeAll(nodeDBService.getLinkedNode(rgv.getCurLocation()));
        log.info("需要释放的路径资源为：" + unlockSet);
        LockNodeManager.removeLockedNodes(unlockSet, rgv);
        log.info("小车执行完成: " + taskDB);
        WCSNodeModule.cleanWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo());
        WCSNodeModule.removeWcsNodeModuleMapCacheLockBy(rgv.getRgvDB().getRgvNo().toString());
        rgv.setCurTask(null);
    }

    /**
     * 计算子车任务路径
     *
     * @param rgv
     * @param taskDB
     * @return
     */
    private Set<Coord> analyzeSonTaskPath(Rgv rgv, TaskDB taskDB) {
        Node node = ((RgvSM) rgv).getSonRgvLocation();
        Set<Coord> pathNodes;
        switch (taskDB.getRgvProgress()) {
            case RGV_SON_OUT_TO_GET:
                if (rgv.getPalletStatus() != PalletStatus.DOWN) {
                    throw new RuntimeException("小车前往取货，托盘需要下降");
                }
            case RGV_SON_LEAVE_GET:
                pathNodes = RgvSonNewTaskHandle.analyzeLineNodes(node, taskDB.selectStartNode());
                break;
            case RGV_SON_OUT_TO_PUT:
                if (rgv.getPalletStatus() != PalletStatus.UP) {
                    throw new RuntimeException("小车前往放货，托盘需要顶升");
                }
            case RGV_SON_LEAVE_PUT:
                pathNodes = RgvSonNewTaskHandle.analyzeLineNodes(node, taskDB.selectEndNode());
                break;
            case RGV_PUT_CARGO:
            case RGV_GET_CARGO:
                pathNodes = new LinkedHashSet<>();
                break;
            case RGV_SON_GOTO_TARGET:
                pathNodes = RgvSonNewTaskHandle.analyzeLineNodes(node, taskDB.selectEndNode());
                break;
            case RGV_TO_START:
                //进度为前往起点，子车需回母车
                pathNodes = RgvSonNewTaskHandle.analyzeLineNodes(node, taskDB.selectStartNode());
                break;
            default:
                throw new RuntimeException("当前任务进度不是子车任务");
        }

        return pathNodes;
    }

    /**
     * 任务进度是否为子车执行
     *
     * @param rgv
     * @param taskDB
     * @param wareHouseDB
     * @return
     */
    private boolean checkIsSonTaskProcess(Rgv rgv, TaskDB taskDB, WareHouseDB wareHouseDB) {
        TaskRgvProgress progress = taskDB.getRgvProgress();
        if (rgv.checkRgvType(RgvType.RGV_SON_MOTHER)) {
            if (progress == TaskRgvProgress.RGV_SON_OUT_TO_GET
                    || progress == TaskRgvProgress.RGV_GET_CARGO
                    || progress == TaskRgvProgress.RGV_SON_LEAVE_GET
                    || progress == TaskRgvProgress.RGV_SON_OUT_TO_PUT
                    || progress == TaskRgvProgress.RGV_PUT_CARGO
                    || progress == TaskRgvProgress.RGV_SON_LEAVE_PUT
                    || progress == TaskRgvProgress.RGV_SON_GOTO_TARGET
            ) {
                //子车任务，母车必须在库内
                if (!RgvSonNewTaskHandle.inWareHouse(rgv.getCurLocation(), wareHouseDB)) {
                    throw new RuntimeException("母车不在库内，任务进度不能为：" + progress + ", 请正确修改任务进度后再恢复任务");
                }
                return true;
            }
        }

        return false;
    }

    /**
     * 检测子车是否在母车内
     *
     * @param rgv
     */
    private void checkIfSonRgvInMother(Rgv rgv) {
        if (!rgv.checkRgvType(RgvType.RGV_SON_MOTHER)) {
            return;
        }
        RgvSM rgvSM = (RgvSM) rgv;
        if (!rgvSM.isRgvSonInMother()) {
            //TODO RGVSM safecheck
            throw new Rgv60TaskException("子车不在母车内，请先把子车开回母车");
        }
    }

    /**
     * 前往起点
     *
     * @param taskDB
     * @param rgv
     * @param toRes
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws InterruptedException
     * @throws IOException
     */
    private void runToStart(TaskDB taskDB, Rgv rgv, ToRes toRes, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB) throws PlcExecuteException, InterruptedException, IOException {
        boolean isFinish = false;
        log.info("小车托盘状态为:" + rgv.getPalletStatus().getDescription());
        checkIfSonRgvInMother(rgv);
        NodeDB curNodeDB = nodeDBService.getNodeDB(rgv.getCurLocation());
        if (!rgv.checkRgvType(RgvType.RGV_FORK)) {
            if (rgv.isHasCargo()) {
                throw new CreatePathFailedException(rgv.getCurLocation(), taskDB.selectStartNode(), "小车托盘未下降，无法取货！");
            }
        }
        if (rgv.checkRgvType(RgvType.FIVE_POINT_FOUR_TL)) {
            if (NodeType.R.equals(curNodeDB.getGridType())
                    && (rgv.getHasPallet() == null || rgv.getHasPallet() == RgvCargoStatus.CARGO_STANDARD
                    || rgv.getHasPallet() == RgvCargoStatus.CARGO_NOT_STANDARD)) {
                throw new CreatePathFailedException(rgv.getCurLocation(), taskDB.selectStartNode(), String.format("小车托盘未下降,或者顶部光电有货，无法执行%s任务！", taskDB.getTaskType().getDescription()));
            }
        }
        log.info("任务" + taskDB + "冲往起点中！");
        List<Integer> layers = new ArrayList<>();
        layers.add(rgv.getCurLocation().getZ());
        layers.add(taskDB.selectStartNode().getZ());
        taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_TO_START);
        long sendTime = System.currentTimeMillis();
        while (!isFinish) {
            try {
                DifFloorRouteFactoryImp routeFactory = new DifFloorRouteFactoryImp(nodeDBService.getNodeByLayer(layers),
                        LockNodeManager.getLockedNodesExceptSelf(rgv), rgv.generateWCSTimeModuleBO(),
                        rgv.getRgvDB().getRgvType());
                List<Route> toLoad = routeFactory.createRoute(rgv.getCurLocation(), taskDB.selectStartNode(), false);
                rgv.setWcsTimeModuleResultBO(WCSTimeModule.calculateRouteListPassTime(toLoad, rgv.generateWCSTimeModuleBO(), rgv.getRgvDB().getRgvNo()));
                taskDB.setToStartTime(rgv.getWcsTimeModuleResultBO().getTotalTime());
                rgv.checkInterrupt("RgvTaskHandle3", sendTime);
                executeRoutes(toLoad, taskDB, rgv, taskDetailDB, wareHouseDB);
                isFinish = true;
            } catch (JammingTimeoutException e) {
                log.info("开始重新规划路径1");
                toRes.initialize(taskDB.getTaskId());
            } catch (OccupyLifterFailException e) {
                log.info("开始重新规划路径2,{}", e.getMessage());
                toRes.initialize(taskDB.getTaskId());
            } catch (MakeWayException e) {
                log.info("开始重新规划路径3,{}", e.getMessage());
                toRes.initialize(taskDB.getTaskId());
            }
        }
    }

    /**
     * 前往终点
     *
     * @param taskDB
     * @param rgv
     * @param toRes
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws InterruptedException
     * @throws IOException
     */
    private void runToEnd(TaskDB taskDB, Rgv rgv, ToRes toRes, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB) throws PlcExecuteException, InterruptedException, IOException {
        boolean isFinish = false;

        if (rgv.checkRgvType(RgvType.RGV_SEER_COMM)) {
            ((Rgv60) rgv).analyzeData();
        }

        log.info("任务" + taskDB + "冲往终点中！");
        log.info("小车托盘状态为:" + rgv.getPalletStatus().getDescription());
        checkIfSonRgvInMother(rgv);
        List<Integer> layers = new ArrayList<>();
        layers.add(rgv.getCurLocation().getZ());
        layers.add(taskDB.selectEndNode().getZ());
        DifFloorRouteFactoryImp routeFactory;
        List<Route> toUnLoad;

        NodeDB curNodeDB = nodeDBService.getNodeDB(rgv.getCurLocation());
        if (TaskType.CHARGE.equals(taskDB.getTaskType()) || TaskType.MOVE_UNLOADED.equals(taskDB.getTaskType())) {
            if (!rgv.checkRgvType(RgvType.RGV_FORK)) {
                if (rgv.isHasCargo()) {
                    throw new CreatePathFailedException(rgv.getCurLocation(), taskDB.selectStartNode(), String.format("小车托盘未下降，无法执行%s任务！", taskDB.getTaskType().getDescription()));
                }
            }
            if (rgv.checkRgvType(RgvType.FIVE_POINT_FOUR_TL)) {
                if (NodeType.R.equals(curNodeDB.getGridType())
                        && (rgv.getHasPallet() == null || rgv.getHasPallet() == RgvCargoStatus.CARGO_STANDARD
                        || rgv.getHasPallet() == RgvCargoStatus.CARGO_NOT_STANDARD)) {
                    throw new CreatePathFailedException(rgv.getCurLocation(), taskDB.selectStartNode(), String.format("小车托盘未下降，无法执行%s任务！", taskDB.getTaskType().getDescription()));
                }
            }
        }
        long sendTime = System.currentTimeMillis();
        int createPathFailedExceptionNum = 0;
        while (!isFinish) {
            // 执行
            try {
                // 入库或移库任务前往终点中前需检测目的位置是否可放，是否需要改变目的终点
                changeEndNodeHandleBeforeRun(taskDB, taskDetailDB);
                routeFactory = new DifFloorRouteFactoryImp(nodeDBService.getNodeByLayer(layers),
                        LockNodeManager.getLockedNodesExceptSelf(rgv), rgv.generateWCSTimeModuleBO(),
                        rgv.getRgvDB().getRgvType());
                toUnLoad = routeFactory.createRoute(rgv.getCurLocation(), taskDB.selectEndNode(), taskDB.getTaskType() == TaskType.MOVE_UNLOADED || taskDB.getTaskType() == TaskType.CHARGE ? false : true);
                if (taskDB.getRgvProgress().equals(TaskRgvProgress.RGV_TO_START)) {
                    befDriveHand(taskDB, taskDetailDB, wareHouseDB);
                }
                if (!rgv.checkRgvType(RgvType.RGV_FORK)) {
                    if ((taskDB.getTaskType().equals(TaskType.IN) || taskDB.getTaskType().equals(TaskType.OUT) || taskDB.getTaskType().equals(TaskType.TRANSFER)) && !rgv.isHasCargo()) {
                        throw new CreatePathFailedException(rgv.getCurLocation(), taskDB.selectStartNode(),
                                "小车托盘未顶升，无法去终点放货！");
                    }
                }
                // 如果是5代车且是出入移，则更新小车身上有货
                if (rgv.checkRgvType(RgvType.FIVE_POINT_FOUR_TL) && (TaskType.IN.equals(taskDB.getTaskType())
                        || TaskType.OUT.equals(taskDB.getTaskType()) || TaskType.TRANSFER.equals(taskDB.getTaskType()))) {
                    rgv.setCargoTaskId(taskDB.getTaskId());
                }
                taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_TO_END);
                //执行前操作
                rgv.setWcsTimeModuleResultBO(WCSTimeModule.calculateRouteListPassTime(toUnLoad, rgv.generateWCSTimeModuleBO(), rgv.getRgvDB().getRgvNo()));
                taskDB.setStartToEndTime(rgv.getWcsTimeModuleResultBO().getTotalTime());
                rgv.checkInterrupt("RgvTaskHandle4", sendTime);
                executeRoutes(toUnLoad, taskDB, rgv, taskDetailDB, wareHouseDB);
                isFinish = true;
            } catch (JammingTimeoutException e) {
                log.info("开始重新规划路径2");
                toRes.initialize(taskDB.getTaskId());
            } catch (PathNodeStatusYException e) {
                log.info("开始重新规划路径4,小车载货路径上货位有货");
                toRes.initialize(taskDB.getTaskId());
            } catch (OccupyLifterFailException e) {
                log.info("开始重新规划路径5,{}", e.getMessage());
                toRes.initialize(taskDB.getTaskId());
            } catch (MakeWayException e) {
                log.info("开始重新规划路径6,{}", e.getMessage());
                toRes.initialize(taskDB.getTaskId());
            } catch (CreatePathFailedException e) {
                log.info("开始重新规划路径7,{}", e.getMessage());
                toRes.initialize(taskDB.getTaskId());
                createPathFailedExceptionNum++;
                if (createPathFailedExceptionNum == 2) {
                    throw e;
                }
            }
        }
        //执行后操作
        if (rgv.checkRgvType(RgvType.RGV_SEER_COMM)) {
            ((Rgv60) rgv).analyzeData();
        }
        aftDriveHand(taskDB, taskDetailDB, wareHouseDB);
    }


    /**
     * 小车取货之后的动作和验证
     *
     * @param taskDB
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws IOException
     */
    private void aftDriveHand(TaskDB taskDB, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB) throws PlcExecuteException, IOException {
        TaskType taskType = taskDB.selectTaskTypeEnum();
        Rgv rgv = ((RgvExecuteThread) Thread.currentThread()).getRgv();
        if (taskType.equals(TaskType.IN) || taskType.equals(TaskType.OUT) || taskType.equals(TaskType.TRANSFER)) {
            if (rgv.checkRgvType(RgvType.RGV_ROLLER) && wareHouseDB.getAgvMode()) {
                analyzeNeedPlcAction(taskDB.selectEndNode(), rgv, false);
            }
            ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.PALLET_DOWN);
            if (rgv.checkRgvType(RgvType.RGV_ROLLER) && wareHouseDB.getAgvMode()) {
                ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.PALLET_DOWN);
            }

            if (rgv.checkRgvType(RgvType.RGV_FORK)) {
                Node backNode = backNode(taskDB);
                WcsUtil.generalTask(null, backNode, rgv, TaskType.RGV_FORK_BACK, taskDBService,
                        CommonUtil.PRIORITY_CODE_MANUAL, this.getClass());
            } else {
                if (taskType == TaskType.OUT) {
                    DeviceBase deviceBase = Plc.deviceLifterCoordMap.get(new Coord(taskDB.selectEndNode().getX(), taskDB.selectEndNode().getY(), taskDB.selectEndNode().getZ()));
                    if (deviceBase != null && deviceBase.getPlcDeviceType().equals(PlcDeviceType.AUTOMATIC_LIFTER)) {
                        if (PlcUnit.getLifterOper(taskDB.selectEndNode()) instanceof TwoDirectionRgvLifterOper) {
                            Node node = taskDB.selectEndNode();
                            List<Integer> list = Collections.singletonList(node.getZ());
                            Node backNode = backNode(taskDB);
                            WcsUtil.generalTask(null, backNode, rgv, TaskType.MOVE_UNLOADED, taskDBService,
                                    CommonUtil.PRIORITY_CODE_MANUAL, this.getClass());
                        }
                    }
                    ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.START_PLC);
                }
            }
        } else if (taskType.equals(TaskType.CHARGE)) {
            ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.START_CHARG);
        }
    }


    private Node backNode(TaskDB taskDB) {
        Node node = taskDB.selectEndNode();
        Node backNode = null;
        int x1 = 1;
        int x2 = 1;
        int y1 = 1;
        int y2 = 1;
        do {
            Node node1 = nodeDBService.getNode(new Node(node.getX() + x1++, node.getY(), node.getZ()));
            if (node1 != null && node1.getNodeType() == NodeType.R) {
                backNode = node1;
            }
            Node node2 = nodeDBService.getNode(new Node(node.getX() - x2++, node.getY(), node.getZ()));
            if (node2 != null && node2.getNodeType() == NodeType.R) {
                backNode = node2;
            }
            Node node3 = nodeDBService.getNode(new Node(node.getX(), node.getY() + y1++, node.getZ()));
            if (node3 != null && node3.getNodeType() == NodeType.R) {
                backNode = node3;
            }
            Node node4 = nodeDBService.getNode(new Node(node.getX(), node.getY() - y2++, node.getZ()));
            if (node4 != null && node4.getNodeType() == NodeType.R) {
                backNode = node4;
            }
            LoggerUtil.info("寻找小车返回点位", this.getClass());
            WcsUtil.delay(1000);
        } while (backNode == null);
        return backNode;
    }

    /**
     * 小车取货之前的动作和验证
     *
     * @param taskDB
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws IOException
     */
    private void befDriveHand(TaskDB taskDB, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB) throws PlcExecuteException, IOException {
        TaskType taskType = taskDB.selectTaskTypeEnum();
        Rgv rgv = ((RgvExecuteThread) Thread.currentThread()).getRgv();
        long sendTime = System.currentTimeMillis();
        if (taskType.equals(TaskType.IN) || taskType.equals(TaskType.OUT) || taskType.equals(TaskType.TRANSFER) || taskType.equals(TaskType.MOVE_LOADED)) {
            //  取货前验证前面那个是否已经拉走
            if (taskType.equals(TaskType.IN)) {
                Node targetNode = nodeDBService.getNodeDB(taskDB.selectStartNode()).getGridType().equals(NodeType.L) ?
                        new Node(taskDB.selectStartNode().getX(), taskDB.selectStartNode().getY(), 1) : taskDB.selectStartNode();
                if (nodeDBService.getNodeDB(taskDB.selectStartNode()).getGridType().equals(NodeType.L)) {
                    taskDB = taskDBService.queryTaskInfoById(taskDB.getTaskId());
                    while (!PlcUnit.isLifterCanGet(targetNode) || taskDB.getPlcProgress() != TaskPlcProgress.ALL_DEVICE_COMPLETE.getProgressNum()) {
                        taskDB = taskDBService.queryTaskInfoById(taskDB.getTaskId());
                        rgv.checkInterrupt("RgvTaskHandle5", sendTime);
                        log.info("设备任务执行进度：" + taskDB + "提升机内部状态为不可取,或者任务Plc状态未完成");
                        try {
                            Thread.sleep(1000);
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        }
                    }
                } else {
                    taskDB = taskDBService.queryTaskInfoById(taskDB.getTaskId());
                    while (!(nodeDBService.isCanGet(new Coord(targetNode)) && nodeDBService.checkLifterLeftOrRightGridStatus(targetNode, NodeStatus.Y)
                            && taskDB.getPlcProgress() == TaskPlcProgress.ALL_DEVICE_COMPLETE.getProgressNum())) {
                        taskDB = taskDBService.queryTaskInfoById(taskDB.getTaskId());
                        rgv.checkInterrupt("RgvTaskHandle6", sendTime);
                        log.info("设备任务执行进度：" + taskDB + "入库口状态为不可取,或者任务Plc状态未完成");
                        try {
                            Thread.sleep(1000);
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        }
                    }
                }
                log.info("设备任务执行完成：" + taskDB + "入库货物已到达入库口");
            }
            ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.PALLET_UP);
            if (rgv.checkRgvType(RgvType.RGV_ROLLER) && wareHouseDB.getAgvMode()) {
                analyzeNeedPlcAction(taskDB.selectStartNode(), rgv, true);
                ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, taskDB.selectEndNode(), null, Action.WAIT_ROLLER_LOAD_OK);
            }
        }
    }

    /**
     * 滚筒车取货调用输送线
     *  @param rgvNode
     * @param rgv
     * @param hasCargo
     */
    private void analyzeNeedPlcAction(Node rgvNode, Rgv rgv, boolean hasCargo) {
        if (!rgv.checkRgvType(RgvType.RGV_ROLLER)) {
            return;
        }

        NodeDB nodeDB = nodeDBService.getNodeDB(rgvNode);
        if (nodeDB.getCanToDown()) {
            stationAction(new Node(nodeDB.getGridx() + 1, nodeDB.getGridy(), nodeDB.getGridz()), rgv, hasCargo);
        }
        if (nodeDB.getCanToUp()) {
            stationAction(new Node(nodeDB.getGridx() - 1, nodeDB.getGridy(), nodeDB.getGridz()), rgv, hasCargo);
        }
        if (nodeDB.getCanToLeft()) {
            stationAction(new Node(nodeDB.getGridx(), nodeDB.getGridy() - 1, nodeDB.getGridz()), rgv, hasCargo);
        }
        if (nodeDB.getCanToRight()) {
            stationAction(new Node(nodeDB.getGridx(), nodeDB.getGridy() + 1, nodeDB.getGridz()), rgv, hasCargo);
        }
    }

    /**
     * 工位转动
     *
     * @param node
     * @param rgv
     * @param hasCargo
     */
    private void stationAction(Node node, Rgv rgv, boolean hasCargo) {
        NodeDB nodeDB = nodeDBService.getNodeDB(node);
        if (nodeDB.getGridType() != NodeType.ST) {
            return;
        }

        LoggerUtil.info("输送线转动准备发送：" + nodeDB + ", 是否取货：" + hasCargo, this.getClass());
        StationRunBean stationRunBean = new StationRunBean();
        stationRunBean.setAgvApoint(nodeDB.getGridId());
        Direction direction = Direction.parseDirection(nodeDB.getDirection());
        if (!hasCargo) {
            if (direction == Direction.IN) {
                direction = Direction.OUT;
            } else if (direction == Direction.OUT) {
                direction = Direction.IN;
            } else {
                LoggerUtil.error("工位转动方向配置错误：" + nodeDB, this.getClass());
                WcsUtil.notification("工位转动方向配置错误：" + nodeDB);
            }
        }
        stationRunBean.setDirection(direction.getDirectionNum());
        long sendTime = System.currentTimeMillis();
        while (!ToUpper.sendTaskStationRun(stationRunBean)) {
            rgv.checkInterrupt("", sendTime);
            WcsFuncUtil.delay(1000);
        }
        LoggerUtil.info("输送线转动发送成功：" + nodeDB + ", 是否取货：" + hasCargo, this.getClass());
    }

    /**
     * 转换工位转动方向
     *
     * @param direction
     * @return
     */
    private Direction analyzeStationDir(Integer direction) {
        return Direction.parseDirection(direction);
    }

    /**
     * 使小车执行已规划的路径
     *
     * @param toLoad
     * @param taskDB
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws InterruptedException
     * @throws IOException
     */
    private void executeRoutes(List<Route> toLoad, TaskDB taskDB, Rgv rgv, TaskDetailDB taskDetailDB,
                               WareHouseDB wareHouseDB) throws PlcExecuteException, InterruptedException, IOException {
        if (rgv.checkRgvType(RgvType.RGV_SON_MOTHER)) {
            if (!((RgvSM) rgv).isRgvSonInMother()) {
                rgvSonNewTaskHandle(taskDB);
            }
        }

        for (Route route : toLoad) {
            if (rgv.getCurLocation().getZ() != route.getStartGrid().getZ()) {
                LifterOper hoistOper = PlcUnit.getLifterOper(route.getStartGrid());
                Lifter lifter = nodeDBService.getLifter(new Node(route.getStartGrid()));
                LifterEventInterface lifterEvent = SystemInit.getLifterEvent(hoistOper);
                //调用提升机载车到目标层
                lifterEvent.rgvNotLoadAssignHoist2(hoistOper, lifter, rgv, taskDB.getPlcId(), route.getStartGrid().getZ());
                log.info("在" + rgv.getCurLocation() + "换层至: " + route.getStartGrid());
                ToRes toRes = ToRes.getInstance(rgv);
                toRes.changeLocation(taskDB.getTaskId(), route.getStartGrid());
            }
            WCSNodeModule.cleanWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo());
            pathHandle.handle(route, taskDB, taskDetailDB, wareHouseDB);
            WCSNodeModule.cleanWcsNodeModuleMapCacheRouteBy(rgv.getRgvDB().getRgvNo());
            Set<Coord> unlockSet = LockNodeManager.getLockCoordByObj(rgv);
            if (taskDB.getTaskType().getTypeNum() == TaskType.CHARGE.getTypeNum()) {
                try {
                    Node node = Node.parseGrid(taskDB.getEndNodeStr());
                    unlockSet.remove(node);
                } catch (Exception e) {
                }
            }
            unlockSet.removeAll(nodeDBService.getLinkedNode(rgv.getCurLocation()));
            log.info("行驶一段路径后需要释放的路径资源为：" + unlockSet);
            LockNodeManager.removeLockedNodes(unlockSet, rgv);
        }
    }

    /**
     * 跨层移库任务
     *
     * @param taskDB      当前任务
     * @param rgv         执行车辆
     * @param toRes       小车res
     * @param wareHouseDB
     * @return
     * @throws PlcExecuteException
     * @throws InterruptedException
     * @throws IOException
     */
    private void lifterCarAnalyze(TaskDB taskDB, Rgv rgv, ToRes toRes, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB) throws PlcExecuteException, InterruptedException, IOException {
        //不是自卸式提升机不执行自卸式提升机流程
        for (DeviceBase deviceBase : Plc.deviceLifterCoordMap.values()) {
            if (!(deviceBase.getPlcDeviceType().equals(PlcDeviceType.AUTOMATIC_LIFTER))) {
                return;
            }
        }
        if (taskDB.getTaskType() == TaskType.TRANSFER) {
            //移库任务起点和终点不在同一层
            if (taskDB.selectStartNode().getZ() != taskDB.selectEndNode().getZ()) {
                TaskDetailDB autoDetail = taskDetailService.getTaskDetail(taskDB.getTaskId(), DetailProcess.AUTO_TRANSFER_TASK, taskDetailDB.getUuid(),
                        rgv.getCurLocationStr(), null, PrecessStatus.CONDUCT, rgv.toString(), null);
                //开始路径规划
                log.info("小车托盘状态为:" + rgv.getPalletStatus().getDescription());
                List<Integer> layers = new ArrayList<>();
                layers.add(taskDB.selectStartNode().getZ());
                layers.add(taskDB.selectEndNode().getZ());
                DifFloorRouteFactoryImp routeFactory = new DifFloorRouteFactoryImp(nodeDBService.
                        getNodeByLayer(layers), LockNodeManager.getLockedNodesExceptSelf(rgv),
                        rgv.generateWCSTimeModuleBO(), rgv.getRgvDB().getRgvType());
                Map<LifterNodeAnalyze.LifterPathNode, Node> lifterPathNodeNodeMap = lifterPathPlan(taskDB, routeFactory);
                Node lifterNode = lifterPathNodeNodeMap.get(LifterNodeAnalyze.LifterPathNode.LIFTER_PATH_NODE);
                LifterOper lifterOper = PlcUnit.getLifterOper(new Coord(lifterNode.getX(), lifterNode.getY(), 1));
                //该提升机类型不是自卸式提升机
                if (lifterOper != null && !(lifterOper instanceof TwoDirectionRgvLifterOper)) {
                    return;
                }
                Node startLifterNode = lifterPathNodeNodeMap.get(LifterNodeAnalyze.LifterPathNode.START_NODE);
                Node endLifterNode = lifterPathNodeNodeMap.get(LifterNodeAnalyze.LifterPathNode.END_NODE);
                TwoDirectionRgvLifterOper twoDirectionRgvLifterOper = (TwoDirectionRgvLifterOper) lifterOper;
                log.info("任务" + taskDB + "自卸式提升机进行移库");
                //起点和终点在同一层
                if (taskDB.selectStartNode().getZ() == taskDB.selectEndNode().getZ()) {
                    LoggerUtil.info("当前移库任务不需换层", this.getClass());
                    if (lifterOper != null) {
                        throw new RuntimeException("提升机路径规划错误");
                    }
                    return;
                }
                //一楼移库时为防止提升机四向车层
                PlcDB lifterDeviceDB = plcDBService.getLifterDeviceDB(PlcType.AUTOMATIC_LIFTER.getValue());
                boolean cargoRgv = lifterDeviceDB.isCargoRgv();
                if (cargoRgv && (taskDB.selectStartNode().getZ() == 1 || taskDB.selectEndNode().getZ() == 1)) {
                    LoggerUtil.info("开始起点为一层的移库任务,任务id-->" + taskDB.getTaskId(), this.getClass());
                    //四向车移库至提升机外
                    rgvMoveToLifterOutside(taskDB, rgv, toRes, taskDetailDB, wareHouseDB, routeFactory, startLifterNode);
                    //四向车移库到提升机外
                    if (taskDB.getRgvProgress().getProgressNum() <= TaskRgvProgress.RGV_INTO_LIFTER.getProgressNum()) {
                        taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_INTO_LIFTER);
                        Node endNode = null;
                        //提升机终点查询
                        if (lifterNode.getX() == endLifterNode.getX()) {
                            if (lifterNode.getY() > endLifterNode.getY()) {
                                endNode = new Node(endLifterNode.getX(), endLifterNode.getY() - 1, endLifterNode.getZ());
                            } else if (lifterNode.getY() < endLifterNode.getY()) {
                                endNode = new Node(endLifterNode.getX(), endLifterNode.getY() + 1, endLifterNode.getZ());
                            }
                        } else if (lifterNode.getY() == lifterNode.getY()) {
                            if (lifterNode.getX() > endLifterNode.getX()) {
                                endNode = new Node(endLifterNode.getX() - 1, endLifterNode.getY(), endLifterNode.getZ());
                            } else if (lifterNode.getX() < endLifterNode.getX()) {
                                endNode = new Node(endLifterNode.getX() + 1, endLifterNode.getY(), endLifterNode.getZ());
                            }
                        } else {
                            throw new RuntimeException("提升机路径规划错误");
                        }
                        if (endNode == null) {
                            throw new RuntimeException("自卸式提升机跨层移库找不到终点");
                        }
                        runExecuteRoutes(rgv, endNode, taskDB, taskDetailDB, toRes, routeFactory, TaskType.MOVE_UNLOADED,
                                wareHouseDB);
                    }
                    if (taskDB.getRgvProgress().getProgressNum() <= TaskRgvProgress.RGV2_MOVE_CARGO.getProgressNum()) {
                        //两向车移库
                        taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV2_MOVE_CARGO);
                        LoggerUtil.info(twoDirectionRgvLifterOper.getDeviceName() + "起点位置：" + startLifterNode + "移货到终点：" + endLifterNode, this.getClass());
                        //锁定提升机起点与终点
                        LifterPathNode.taskBefore(taskDB);
                        //用于中断提升机任务
                        Plc.PLC_INTERRUPT_CACHE.put(taskDB.getTaskId().toString(), "移库任务");
                        lifterOperMoveCargo(taskDB, TaskRgvProgress.RGV_LEAVE_LIFTER, startLifterNode, endLifterNode, twoDirectionRgvLifterOper, taskDetailDB);
                    }
                    //四响车接货
                    if (taskDB.getRgvProgress().getProgressNum() <= TaskRgvProgress.RGV_LEAVE_LIFTER.getProgressNum()) {
                        LoggerUtil.info(rgv.getRgvName() + "移库到终点：" + endLifterNode, this.getClass());
                        taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_LEAVE_LIFTER);
                        rgvLeaveLifter(rgv, endLifterNode, taskDB, taskDetailDB, toRes, routeFactory, TaskRgvProgress.RGV_LEAVE_LIFTER,
                                twoDirectionRgvLifterOper, wareHouseDB);
                    }
                } else {
                    //自卸式提升机任务任务恢复任务状态不等于前往终点时开始流程恢复
                    if (taskDB.getRgvProgress().getProgressNum() != TaskRgvProgress.RGV_TO_END.getProgressNum()) {
                        //四向车移库至提升机外
                        rgvMoveToLifterOutside(taskDB, rgv, toRes, taskDetailDB, wareHouseDB, routeFactory, startLifterNode);
                        //四向车进入提升机
                        if (taskDB.getRgvProgress().getProgressNum() <= TaskRgvProgress.RGV_INTO_LIFTER.getProgressNum()) {
                            LoggerUtil.info(rgv.getRgvName() + "进入" + lifterOper.getDeviceName(), this.getClass());
                            taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_INTO_LIFTER);
                            //提升机换向为四向车层
                            rgvIntoLifter(rgv, lifterNode, taskDB, taskDetailDB, toRes, routeFactory, TaskType.MOVE_UNLOADED, twoDirectionRgvLifterOper, TaskRgvProgress.RGV2_MOVE_CARGO, wareHouseDB);
                        }
                        //两向车移库
                        if (taskDB.getRgvProgress().getProgressNum() <= TaskRgvProgress.RGV2_MOVE_CARGO.getProgressNum()) {
                            LoggerUtil.info(twoDirectionRgvLifterOper.getDeviceName() + "起点位置：" + startLifterNode + "移货到终点：" + endLifterNode, this.getClass());
                            taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV2_MOVE_CARGO);
                            //锁定提升机起点与终点
                            LifterPathNode.taskBefore(taskDB);
                            //用于中断提升机任务
                            Plc.PLC_INTERRUPT_CACHE.put(taskDB.getTaskId().toString(), "移库任务");
                            lifterOperMoveCargo(taskDB, TaskRgvProgress.RGV_LEAVE_LIFTER, startLifterNode, endLifterNode, twoDirectionRgvLifterOper, taskDetailDB);
                        }
                        //四向车移库到提升机外
                        if (taskDB.getRgvProgress().getProgressNum() <= TaskRgvProgress.RGV_LEAVE_LIFTER.getProgressNum()) {
                            //提升机换层到四向车层
                            LoggerUtil.info(rgv.getRgvName() + "移库到终点：" + endLifterNode, this.getClass());
                            taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_LEAVE_LIFTER);
                            rgvLeaveLifter(rgv, endLifterNode, taskDB, taskDetailDB, toRes, routeFactory, TaskRgvProgress.RGV_LEAVE_LIFTER,
                                    twoDirectionRgvLifterOper, wareHouseDB);
                        }
                    }
                }
                taskDetailService.updateTaskDetailCompleteTime(autoDetail);
            }
        }
    }

    /**
     * 四向车移库至提升机外
     *
     * @param taskDB
     * @param rgv
     * @param toRes
     * @param taskDetailDB
     * @param wareHouseDB
     * @param routeFactory
     * @param startLifterNode
     * @throws InterruptedException
     * @throws IOException
     */
    private void rgvMoveToLifterOutside(TaskDB taskDB, Rgv rgv, ToRes toRes, TaskDetailDB taskDetailDB, WareHouseDB wareHouseDB, DifFloorRouteFactoryImp routeFactory, Node startLifterNode) throws InterruptedException, IOException {
        if (taskDB.getRgvProgress().getProgressNum() <= TaskRgvProgress.RGV_RUN_TO_OUT_LIFTER.getProgressNum()) {
            LoggerUtil.info(rgv.getRgvName() + "移库到" + startLifterNode, this.getClass());
            taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_RUN_TO_OUT_LIFTER);
            rgvRunToOutLifter(rgv, startLifterNode, taskDB, taskDetailDB, toRes, routeFactory, TaskType.TRANSFER, TaskRgvProgress.RGV_INTO_LIFTER, wareHouseDB);
        }
    }

    /**
     * 提升机节点路径规划
     *
     * @param taskDB       当前任务
     * @param routeFactory 路径规划
     * @return
     */
    private Map<LifterNodeAnalyze.LifterPathNode, Node> lifterPathPlan(TaskDB taskDB, DifFloorRouteFactoryImp routeFactory) {
        TaskDB task = taskDBService.getTaskDBById(taskDB.getTaskId());
        LifterNodeAnalyze transTaskAnalyze;
        Map<LifterNodeAnalyze.LifterPathNode, Node> lifterPathNodeNodeMap = new HashMap<>();
        if (task.getLifterPath() == null) {
            List<Route> toUnLoad = routeFactory.createRoute(taskDB.selectStartNode(), taskDB.selectEndNode(),
                    taskDB.getTaskType() == TaskType.MOVE_UNLOADED ? false : true);
            transTaskAnalyze = new LifterNodeAnalyze(toUnLoad, taskDB);
            lifterPathNodeNodeMap = transTaskAnalyze.getLifterPathNodeMap();
            taskDB.setLifterPath(
                    lifterPathNodeNodeMap.get(LifterNodeAnalyze.LifterPathNode.START_NODE) + ";"
                            + lifterPathNodeNodeMap.get(LifterNodeAnalyze.LifterPathNode.LIFTER_PATH_NODE) + ";"
                            + lifterPathNodeNodeMap.get(LifterNodeAnalyze.LifterPathNode.END_NODE)
            );
            taskDBService.updateTaskById(taskDB);
        } else {
            String lifterPath = taskDB.getLifterPath();
            String[] split = lifterPath.split(";");
            lifterPathNodeNodeMap.put(LifterNodeAnalyze.LifterPathNode.START_NODE, nodeDBService.getNode(Node.parseGrid(split[0])));
            lifterPathNodeNodeMap.put(LifterNodeAnalyze.LifterPathNode.LIFTER_PATH_NODE, nodeDBService.getNode(Node.parseGrid(split[1])));
            lifterPathNodeNodeMap.put(LifterNodeAnalyze.LifterPathNode.END_NODE, nodeDBService.getNode(Node.parseGrid(split[2])));
        }
        return lifterPathNodeNodeMap;
    }


    /**
     * 四向车离开提升机
     *
     * @param rgv
     * @param endNode
     * @param taskDB
     * @param toRes
     * @param routeFactory
     * @param progress
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws InterruptedException
     * @throws IOException
     */
    private void rgvLeaveLifter(Rgv rgv, Node endNode, TaskDB taskDB, TaskDetailDB taskDetailDB, ToRes toRes,
                                DifFloorRouteFactoryImp routeFactory, TaskRgvProgress progress,
                                TwoDirectionRgvLifterOper twoDirectionRgvLifterOper, WareHouseDB wareHouseDB)
            throws PlcExecuteException, InterruptedException, IOException {
        long sendTime = System.currentTimeMillis();
        runExecuteRoutes(rgv, endNode, taskDB, taskDetailDB, toRes, routeFactory, TaskType.MOVE_UNLOADED, wareHouseDB);
        while (!(rgv.getCurLocation().equals(endNode) && !twoDirectionRgvLifterOper.isHaveShuttle())) {
            rgv.checkInterrupt("rgvIntoLifter --> runExecurRoutes", sendTime);
            LoggerUtil.info("等待" + rgv.getRgvName() + "出提升机," + "小车当前坐标：" + rgv.getCurLocation() +
                    ", 提升机内是否有四向车：" + twoDirectionRgvLifterOper.isHaveShuttle(), this.getClass());
            WcsUtil.delay(1000);
        }
        if (rgv.getCurLocation().equals(endNode)) {
            taskDBService.updateTaskRgvProgress(taskDB, progress);
            ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.PALLET_UP);
            nodeDBService.updateNodeDBStatus(endNode, NodeStatus.N.toString());
        } else {
            throw new RuntimeException(rgv.getRgvName() + "当前位置错误，当前位置：" + rgv.getCurLocation() +
                    ", 理论位置：" + endNode);
        }
    }

    /**
     * 提升机两向车开始移货
     *
     * @param taskdb
     * @param progress
     * @param startLifterNode
     * @param endLifterNode
     * @param twoDirectionRgvLifterOper
     * @throws PlcExecuteException
     * @throws IOException
     */
    private void lifterOperMoveCargo(TaskDB taskdb, TaskRgvProgress progress,
                                     Node startLifterNode, Node endLifterNode,
                                     TwoDirectionRgvLifterOper twoDirectionRgvLifterOper,
                                     TaskDetailDB taskDetailDB) throws PlcExecuteException, IOException {
        LifterEventInterface lifterEvent = SystemInit.getLifterEvent(twoDirectionRgvLifterOper);
        lifterEvent.palletTask(twoDirectionRgvLifterOper, null, taskdb, null, startLifterNode.getZ(),
                endLifterNode.getZ(), PlcUnit.judgeLeftOrRightNode(startLifterNode),
                PlcUnit.judgeLeftOrRightNode(endLifterNode), taskDetailDB);
        taskDBService.updateTaskRgvProgress(taskdb, progress);
    }

    /**
     * 四向车进入提升机内
     *
     * @param rgv
     * @param endNode
     * @param taskDB
     * @param toRes
     * @param routeFactory
     * @param taskType
     * @param progress
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws InterruptedException
     * @throws IOException
     */
    private void rgvIntoLifter(Rgv rgv, Node endNode, TaskDB taskDB, TaskDetailDB taskDetailDB, ToRes toRes, DifFloorRouteFactoryImp routeFactory,
                               TaskType taskType, TwoDirectionRgvLifterOper twoDirectionRgvLifterOper, TaskRgvProgress progress, WareHouseDB wareHouseDB)
            throws PlcExecuteException, InterruptedException, IOException {
        runExecuteRoutes(rgv, endNode, taskDB, taskDetailDB, toRes, routeFactory, taskType, wareHouseDB);
        long sendTime = System.currentTimeMillis();
        while (!(rgv.getCurLocation().equals(endNode) && twoDirectionRgvLifterOper.isHaveShuttle())) {
            rgv.checkInterrupt("rgvIntoLifter --> runExecurRoutes", sendTime);
            LoggerUtil.info("等待" + rgv.getRgvName() + "进入提升机," + "小车当前坐标：" + rgv.getCurLocation() +
                    ", 提升机内是否有四向车：" + twoDirectionRgvLifterOper.isHaveShuttle(), this.getClass());
            WcsUtil.delay(1000);
        }
        taskDBService.updateTaskRgvProgress(taskDB, progress);
    }

    /**
     * 四向车移库到提升机外接驳点
     *
     * @param rgv
     * @param endNode
     * @param taskDB
     * @param toRes
     * @param routeFactory
     * @param taskType
     * @param progress
     * @param wareHouseDB
     * @throws PlcExecuteException
     * @throws InterruptedException
     * @throws IOException
     */
    private void rgvRunToOutLifter(Rgv rgv, Node endNode, TaskDB taskDB, TaskDetailDB taskDetailDB, ToRes toRes, DifFloorRouteFactoryImp routeFactory,
                                   TaskType taskType, TaskRgvProgress progress, WareHouseDB wareHouseDB)
            throws PlcExecuteException, InterruptedException, IOException {
        //托盘顶升
        ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.PALLET_UP);
        //移库到提升机外面
        runExecuteRoutes(rgv, endNode, taskDB, taskDetailDB, toRes, routeFactory, taskType, wareHouseDB);
        //下降
        ActionHandle.actionHandle(taskDB, taskDetailDB, rgv, null, null, Action.PALLET_DOWN);
        taskDBService.updateTaskRgvProgress(taskDB, progress);
    }

    /**
     * 执行选定路线任务
     *
     * @param rgv          当前执行任务rgv
     * @param endNode      目标终点
     * @param taskDB       正在执行的任务
     * @param toRes        小车通讯Res
     * @param routeFactory 路线工厂类
     * @param taskType     任务类型
     * @param wareHouseDB
     * @throws PlcExecuteException  plc异常
     * @throws InterruptedException 中断异常
     * @throws IOException
     */
    private void runExecuteRoutes(Rgv rgv, Node endNode, TaskDB taskDB, TaskDetailDB taskDetailDB, ToRes toRes,
                                  DifFloorRouteFactoryImp routeFactory, TaskType taskType, WareHouseDB wareHouseDB)
            throws PlcExecuteException, InterruptedException, IOException {
        List<Route> toUnLoad = routeFactory.createRoute(rgv.getCurLocation(), endNode,
                taskType == TaskType.MOVE_UNLOADED || taskType == TaskType.CHARGE ? false : true);
        long sendTime = System.currentTimeMillis();
        while (true) {
            try {
                rgv.checkInterrupt("RgvTaskHandle 执行选定路线任务", sendTime);
                executeRoutes(toUnLoad, taskDB, rgv, taskDetailDB, wareHouseDB);
                break;
            } catch (JammingTimeoutException e) {
                log.info("开始重新规划路径 执行选定路线任务");
                toRes.initialize(taskDB.getTaskId());
                toUnLoad = routeFactory.createRoute(rgv.getCurLocation(), endNode,
                        taskType == TaskType.MOVE_UNLOADED || taskType == TaskType.CHARGE ? false : true);
            } catch (OccupyLifterFailException e) {
                log.info("开始重新规划路径 执行选定路线任务,{}", e.getMessage());
                toRes.initialize(taskDB.getTaskId());
                List<Integer> layers = new ArrayList<>();
                layers.add(rgv.getCurLocation().getZ());
                layers.add(taskDB.selectStartNode().getZ());
                routeFactory = new DifFloorRouteFactoryImp(nodeDBService.getNodeByLayer(layers),
                        LockNodeManager.getLockedNodesExceptSelf(rgv), rgv.generateWCSTimeModuleBO(),
                        rgv.getRgvDB().getRgvType());
                toUnLoad = routeFactory.createRoute(rgv.getCurLocation(), endNode,
                        taskType == TaskType.MOVE_UNLOADED || taskType == TaskType.CHARGE ? false : true);
            }
        }
    }

    /**
     * 入库任务和移库任务改变任务终点
     *
     * @param taskDB
     */
    private void changeEndNodeHandleBeforeRun(TaskDB taskDB, TaskDetailDB endDet) {
        //入库任务或者移库任务判断是否设置改变终点
        if (wareHouseDBService.getIsChangeEnd() && (taskDB.getTaskType().equals(TaskType.IN) || taskDB.getTaskType().equals(TaskType.TRANSFER))) {
            //检查终点位置是否是单巷道最深位，不是则需要更换终点位置。
            Node deepNode = nodeDBService.getEndIsDeepNode(Node.parseGrid(taskDB.getStartNodeStr()), Node.parseGrid(taskDB.getEndNodeStr()), taskDB.getTaskType());
            if (deepNode != null && !taskDB.selectEndNode().equals(deepNode)) {
                log.info("任务" + taskDB + "改变终点中！原终点：{},改变后终点：{}，重新规划路径", taskDB.getEndNodeStr(), deepNode);
                TaskDetailDB taskDetailDB = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.CHANGE_END, endDet.getUuid(), taskDB.getEndNodeStr(), deepNode.toString(),
                        PrecessStatus.CONDUCT, null, null);
                //更新任务的终点为新的未放货的最深位，重新规划路径
                taskDBService.updateTaskEndNodeStr(taskDB, deepNode);
                taskDetailService.updateTaskDetailCompleteTime(taskDetailDB);
            }
            if (deepNode == null) {
                log.info("任务{}，单巷道货位已满货，终点：{}，不能再放货,或者货位已被WMS禁用", taskDB, taskDB.getEndNodeStr());

                throw new SinglenRoadNodeFullException(String.format("任务%s，单巷道货位已满货，终点：%s，不能再放货,或者货位已被WMS禁用", taskDB.getTaskId(), taskDB.getEndNodeStr()));
            }
        }
    }

    /**
     * 子车任务类型处理
     *
     * @param taskDB
     */
    public void rgvSonNewTaskHandle(TaskDB taskDB) {
        Rgv rgv = taskDB.selectProcessingRgvPojo();
        if (taskDB.getTaskType() == TaskType.RGV_SON_MOVE_UNLOADED || taskDB.getTaskType() == TaskType.MOVE_LOADED || taskDB.getTaskType() == TaskType.TRANSFER) {
            Set<Coord> coords = RgvSonNewTaskHandle.analyzeLineNodes(((RgvSM) rgv).getSonRgvLocation(),
                    taskDB.selectEndNode());

            if (coords.size() >= 2) {
                ToRes toRes = ToRes.getInstance(rgv);
                Integer resTaskNo = toRes.getAutoIncrementTaskNum();
                RgvSonNewTaskHandle.sonRgvNewRun((RgvSM) rgv, taskDB, resTaskNo, coords, nodeDBService, taskDBService);
            }
            taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_COMPLETE);
            taskDBService.updateTaskStatus(taskDB, TaskStatus.FINISH);
            rgv.setCurTask(null);
        } else {
            throw new Rgv60TaskException("不支持的子车任务：" + taskDB.getTaskType());
        }
    }
}
